#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>

ros::Publisher pub ;
void imuctl_callback(sensor_msgs::Imu msgs)
{
  if (msgs.orientation_covariance[0] < 0) return;
  tf::Quaternion quaternion(
    msgs.orientation.x,
    msgs.orientation.y,
    msgs.orientation.y,
    msgs.orientation.w
  );

  double roll,  pitch, yaw;
  tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
  roll = roll * 180 / M_PI;
  pitch = pitch *180 / M_PI;
  yaw = yaw *180 /M_PI;

 ROS_INFO("滚转  =  %.0f  俯仰 = %.0f  朝向 = %.0f ", roll, pitch, yaw);
 double target_yaw = 90;

 double diff_angle = target_yaw - yaw;
 geometry_msgs::Twist vel_cmd;
 vel_cmd.angular.z = diff_angle * 0.5;
 pub.publish(vel_cmd);

}


int main(int argc, char *argv[])
{
  setlocale(LC_ALL,   "zh_CN.utf-8");
  ros::init(argc, argv, "imuctl_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("/imu/data", 10, imuctl_callback);
 pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

  ros::spin();
    return 0;
}